﻿#include <glog/logging.h>
#include <QDateTime>
#include <qmath.h>

#include <opencv2/opencv.hpp>

#include "constant.h"
#include "manager/config_manager.h"
#include "detect/result_manager.h"
#include "manager/calibrate_manager.h"
#include "manager/mqtt_manager.h"

ResultManager *ResultManager::get_instance() {
    static ResultManager instance;
    return &instance;
}

ResultManager::ResultManager() {
    m_next_result_id = 0;
    sp_tracker = QSharedPointer<KCFTracker>(new KCFTracker(true, false, true, false));

    ptr_kcf_thread = new KCFThread();
    connect(this, SIGNAL(sig_update_kcf(QSharedPointer<Mat>, bool)),
            ptr_kcf_thread, SLOT(on_kcf_update(QSharedPointer<Mat>, bool)));
}

void ResultManager::init()
{
    ptr_kcf_thread->start();
}

ResultManager::~ResultManager() {
    if (ptr_kcf_thread != NULL) {
        delete ptr_kcf_thread;
        ptr_kcf_thread = NULL;
    }
}

void ResultManager::detect(QSharedPointer<Mat> sp_frame, std::vector<AlgorithmResultEntity> &result) {
    LOG(INFO) << "ResultManager | detect | result_count " << result.size();
    clear_match_status();
    m_mutex.lock();
    AppConfig *app_config = ConfigManager::getInstance()->get_app_config();

    for (std::vector<AlgorithmResultEntity>::iterator kit = m_result_buffer.begin(); kit != m_result_buffer.end(); )  {
        qint64 current_time = QDateTime::currentMSecsSinceEpoch();
        if (current_time - kit->timestamp > SSD_RESULT_TIMEOUT_MS) {
            kit = m_result_buffer.erase(kit);
        } else {
            if (app_config->show_detect == true) {
                std::ostringstream oss_result;
                oss_result << kit->result_id;
                cv::rectangle(*sp_frame, kit->kcf_roi, cvScalar(0, 255, 0), 2);
                cv::putText(*sp_frame, oss_result.str(), cvPoint(kit->kcf_roi.x, kit->kcf_roi.y),
                            cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(0, 255, 0), 1);
            }
            kit++;
        }
    }

    for (std::vector<AlgorithmResultEntity>::iterator it = result.begin(); 
        it != result.end(); ++it) { 
        double similar = 0;
        std::ostringstream oss;
        // 更新缓冲区并返回更新的行人ID号
        int result_id = update_result_buffer(&(*it), similar);
        if (result_id < 0) {
            it->result_id = get_next_result_id();
            add_result_buffer(&(*it));
            oss << it->result_id << "/0";
        } else {
            it->result_id = result_id;
            oss << it->result_id << "/" << similar;
        }
        // 显示框图和识别结果
        if (app_config->show_detect == true) {
            cv::rectangle(*sp_frame, cv::Point(it->xmin, it->ymin),
                         cv::Point(it->xmax, it->ymax), cvScalar(0, 0, 255), 2);
            cv::putText(*sp_frame, oss.str(), cvPoint(it->xcenter, it->ycenter),
                        cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(0, 0, 255), 1);
        }
    }
    m_mutex.unlock();
    MqttManager::getInstance()->send_pedestrian(&result);
}

int ResultManager::get_next_result_id() {
    if (m_next_result_id < MAX_RESULT_ID) {
        return m_next_result_id++;
    } else {
        m_next_result_id = 0;
        return m_next_result_id;
    }
}

void ResultManager::update_buffer_kcf_async(QSharedPointer<Mat> sp_frame, bool is_ssd_frame)
{
    // LOG(INFO) << "ResultManager | update_buffer_kcf_async";
    emit sig_update_kcf(sp_frame, is_ssd_frame);
}

void ResultManager::update_buffer_kcf(cv::Mat &frame, bool is_ssd_frame) {
    m_mutex.lock();
    // LOG(INFO) << "ResultManager | update_buffer_kcf | is_ssd_frame " << is_ssd_frame;
    for (std::vector<AlgorithmResultEntity>::iterator it = m_result_buffer.begin();
        it != m_result_buffer.end(); ++it) {
        it->kcf_roi = it->sp_tracker->update(frame);
        if (is_ssd_frame) {
            it->kcf_ssd_roi = it->kcf_roi;
            LOG(INFO) << "ResultManager | update_buffer_kcf | " << it->kcf_ssd_roi;
        }
    }
    m_mutex.unlock();
}

int ResultManager::update_result_buffer(AlgorithmResultEntity *result, double &similar) {
    double max_value = 0.0f;
    static cv::Mat similarity;
    std::vector<AlgorithmResultEntity>::iterator ptr_max_value;
    // 计算转换后的坐标
    std::vector<cv::Point2f> v_points;
    v_points.push_back(cv::Point2f(result->xfooter, result->yfooter));
    std::vector<cv::Point2f> v_cali = CalibrateManager::get_instance()->get_trans_ponits(&v_points);
    result->cali_x = v_cali[0].x;
    result->cali_y = v_cali[0].y;
    LOG(INFO) << "ResultManager | update_result_buffer | calibrte "
             << result->xfooter << result->yfooter<< "--->"
             << result->cali_x << result->cali_y;
    // 查找缓冲区中最大的匹配
    for (std::vector<AlgorithmResultEntity>::iterator it = m_result_buffer.begin(); 
        it != m_result_buffer.end(); ++it) {
        if (it->is_match == true) {
            continue;
        }
        double mag_r = 0;
        try {
            cv::Rect result_rect(result->xmin, result->ymin, result->width, result->height);
            mag_r = (result_rect & it->kcf_ssd_roi).area();
            LOG(INFO) << "ResultManager | update_result_buffer | calc match result_point("
                     << result->xmin << result->ymin << result->width << result->height<< "), kcf_point("
                     << (it->kcf_ssd_roi).x << (it->kcf_ssd_roi).y
                     << (it->kcf_ssd_roi).width << (it->kcf_ssd_roi).height << ")"
                     << "area" << mag_r << "buffer_id" << it->result_id;
        } catch (...) {
            LOG(INFO) << "ResultManager | update_result_buffer | exception";
            continue;
        }
        if (mag_r > max_value) {
            max_value = mag_r;
            ptr_max_value = it;
        }
    }
    if (max_value >= TEMPLATE_THRESHOLD) {
        LOG(INFO) << "ResultManager | update_result_buffer | >>> find match result_point("
                 << result->xmin << result->ymin << result->width << result->height<< "), kcf_point("
                 << (ptr_max_value->kcf_ssd_roi).x << (ptr_max_value->kcf_ssd_roi).y
                 << (ptr_max_value->kcf_ssd_roi).width << (ptr_max_value->kcf_ssd_roi).height << ")"
                 << "area" << max_value << "buffer_id" << ptr_max_value->result_id << "<<<";
        // 更新模型并更新缓冲区
        result->is_match = true;
        result->result_id = ptr_max_value->result_id;
        result->speed = get_speed(&(*ptr_max_value), result);
        result->orientation = get_orientation(&(*ptr_max_value), result);
        result->mass = get_mass(result);
        *ptr_max_value = *result;
        // 返回匹配的结果值
        similar = max_value;
        // 将最新的匹配移动至缓冲区的头部
        std::swap(*ptr_max_value, *(m_result_buffer.begin()));
        return result->result_id;
    } else {
        return -1;
    }
}

void ResultManager::add_result_buffer(AlgorithmResultEntity *result) {
    m_result_buffer.insert(m_result_buffer.begin(), *result);
    if (m_result_buffer.size() > MAX_RESULT_BUFFER_SIZE) {
        m_result_buffer.pop_back();
    }
}

void ResultManager::clear_match_status() {
    LOG(INFO) << "ResultManager | clear_match_status";
    for (std::vector<AlgorithmResultEntity>::iterator it = m_result_buffer.begin(); 
        it != m_result_buffer.end(); ++it) {
        it->is_match = false;
    }
}

double ResultManager::get_speed(AlgorithmResultEntity *point1, AlgorithmResultEntity *point2) {
    cv::Point cv_point1(point1->cali_x, point2->cali_y);
    cv::Point cv_point2(point2->cali_x, point2->cali_y);
    cv::Point diff = cv_point2 - cv_point1;
    double distance = cv::sqrt(diff.x * diff.x + diff.y * diff.y) / 100.0;
    double duration = (point2->timestamp - point1->timestamp) / 1000.0;
    return distance / duration;
}

int ResultManager::get_mass(AlgorithmResultEntity *point) {
    int mass = point->area;
    if (mass > 80) {
        mass = 80;
    }
    if (mass < 40) {
        mass = 40;
    }
    return mass;
}

int ResultManager::get_orientation(AlgorithmResultEntity *point1, AlgorithmResultEntity *point2) {
    double angle = qAtan2(point2->cali_y - point1->cali_y, point2->cali_x - point1->cali_x);
    double result = angle * 180 / 3.14;
    if (result < 0) {
        result += 360;
    }
    return result;
}
